#include "CollisionDetector.h"

bool CollisionDetector::collide(GameObject* object1, GameObject* object2) {
    bool result = false;

    if(object1->hasCollider() && object2->hasCollider()) {
        if(collide(object1->getCollider(), object2->getCollider())) {
            result = true;
        }
    }

    for(std::vector<GameObject*>::iterator i = object1->getChildren()->begin(); i != object1->getChildren()->end(); ++i) {
        if(collide((*i), object2)) {
            result = true;
        }
    }

    for(std::vector<GameObject*>::iterator i = object2->getChildren()->begin(); i != object2->getChildren()->end(); ++i) {
        if(collide((*i), object1)) {
            result = true;
        }
    }

    if(result) {
        object1->onCollision(object2);
        object2->onCollision(object1);
    }

    return result;
}

bool CollisionDetector::collide(Collider* collider1, Collider* collider2) {
    if(SphereCollider* sphere1 = dynamic_cast<SphereCollider*>(collider1)) {
        if(SphereCollider* sphere2 = dynamic_cast<SphereCollider*>(collider2)) {
            float distance = glm::distance(sphere1->getParent()->getAbsolutePosition(), sphere2->getParent()->getAbsolutePosition());

            return (distance < sphere1->getRadius() + sphere2->getRadius());
        }
    }

    return false;
}

bool CollisionDetector::collide(MeshCollider* mesh, SphereCollider* sphere, glm::vec3& point, glm::vec3& normal, float& penetration) {
    //Player transformation
    glm::mat4 trans = mesh->getParent()->getTransform();

    //Player location
    glm::vec3 sphereLocation = sphere->getParent()->getLocation();

    //calculate center of player
    glm::vec3 v(sphereLocation.x - trans[3][0], sphereLocation.y - trans[3][1], sphereLocation.z - trans[3][2]);
    float x = v.x * trans[0][0] + v.y * trans[0][1] + v.z * trans[0][2];
    float y = v.x * trans[1][0] + v.y * trans[1][1] + v.z * trans[1][2];
    float z = v.x * trans[2][0] + v.y * trans[2][1] + v.z * trans[2][2];

    glm::vec3 center = glm::vec3(x, y, z);

    for(unsigned int i = 0; i < mesh->getParent()->getMesh()->size(); i += 3) {
        glm::vec3 a = mesh->getParent()->getMesh()->getVertex(i);
        glm::vec3 b = mesh->getParent()->getMesh()->getVertex(i + 1);
        glm::vec3 c = mesh->getParent()->getMesh()->getVertex(i + 2);

        // Source we used for the triangle - sphere detection
        // http://www.planet-source-code.com/vb/scripts/ShowCode.asp?txtCodeId=10290&lngWId=3
        glm::vec3 ab = b - a;
        glm::vec3 ac = c - a;
        glm::vec3 bc = c - b;

        // Compute parametric position s for projection P' of P on AB,
        // P' = A + s*AB, s = sNominator/(sNominator+sDenominator)
        float sNominator = glm::dot(center - a, ab);
        float sDenominator = glm::dot(center - b, a - b);

        // Compute parametric position t for projection P' of P on AC,
        // P' = A + t*AC, s = tNominator/(tNominator+tDenominator)
        float tNominator = glm::dot(center - a, ac);
        float tDenominator = glm::dot(center - c, a - c);

        if(sNominator <= 0.0f && tNominator <= 0.0f) {
            point = a; // Vertex region early out
        }

        // Compute parametric position u for projection P' of P on BC,
        // P' = B + u*BC, u = uNominator/(uNominator+uDenominator)
        float uNominator = glm::dot(center - b, bc);
        float uDenominator = glm::dot(center - c, b - c);

        if(sDenominator <= 0.0f && uNominator <= 0.0f) {
            point = b; // Vertex region early out
        }

        if(tDenominator <= 0.0f && uDenominator <= 0.0f) {
            point = c; // Vertex region early out
        }

        // P is outside (or on) AB if the triple scalar product [N PA PB] <= 0
        glm::vec3 n = glm::cross(b - a, c - a);
        float vc = glm::dot(n, glm::cross(a - center, b - center));

        // If P outside AB and within feature region of AB,
        // return projection of P onto AB
        if(vc <= 0.0f && sNominator >= 0.0f && sDenominator >= 0.0f) {
            point = a + sNominator / (sNominator + sDenominator) * ab;
        }

        // P is outside (or on) BC if the triple scalar product [N PB PC] <= 0
        float va = glm::dot(n, glm::cross(b - center, c - center));

        // If P outside BC and within feature region of BC,
        // return projection of P onto BC
        if(va <= 0.0f && uNominator >= 0.0f && uDenominator >= 0.0f) {
            point = b + uNominator / (uNominator + uDenominator) * bc;
        }

        // P is outside (or on) CA if the triple scalar product [N PC PA] <= 0
        float vb = glm::dot(n, glm::cross(c - center, a - center));

        // If P outside CA and within feature region of CA,
        // return projection of P onto CA
        if(vb <= 0.0f && tNominator >= 0.0f && tDenominator >= 0.0f) {
            point = a + tNominator / (tNominator + tDenominator) * ac;
        }

        // P must project inside face region. Compute Q using barycentric coordinates
        float u = va / (va + vb + vc);
        float v = vb / (va + vb + vc);
        float w = 1.0f - u - v;
        point = u * a + v * b + w * c;

        // Sphere and triangle intersect if the (squared) distance from sphere
        // center to point p is less than the (squared) sphere radius
        glm::vec3 distance = point - sphere->getParent()->getLocation();

        if(glm::dot(distance, distance) >= sphere->getRadius() * sphere->getRadius()) {
            normal = glm::normalize(glm::cross(b - a, c - a));
            penetration = sphere->getRadius() - (float) sqrt(glm::length2(center - point));
            point = glm::vec3(mesh->getParent()->getTransform() * glm::vec4(point, 1.0f));

            return true;
        }
    }

    return false;
}
